package cn.edu.uestc.indoorlocation.algorithm.kalman;

import Jama.Matrix;

/**
 * EKF实现
 * @author wensen
 *
 */
public class EKF {

	/**
	 * 当前的偏向角、x坐标、y坐标
	 */
	private Matrix currentX;
	/**
	 * 上一步的偏向角、x坐标、y坐标
	 */
	private Matrix preX;
	
	/**
	 * 上一步的协方差结果
	 */
	private Matrix preP = new Matrix(3,3);
	/**
	 * 当前步的协方差结果
	 */
	private Matrix currentP = new Matrix(3, 3);
	
	/**
	 * FK, 即对currentX求偏导后的值
	 */
	private Matrix Fk = Matrix.identity(3, 3);
	
	/**
	 * 状态噪声
	 */
	private Matrix Qk = new Matrix(3, 3);
	
	/**
	 * 对观测值求偏导，即分别对观测航向角（手机可采集），x、y坐标(由指纹定位得到)
	 * 初始化为一个3*3的单位矩阵
	 */
	private Matrix H = Matrix.identity(3, 3);
	/**
	 * 3*3的单位矩阵
	 */
	private Matrix I3 = Matrix.identity(3, 3);
	

	public EKF() {}
	
	/**
	 * 初始化值使用指纹定位得到
	 */
	private void init() {
		
//		preX = Fingerprint.position();
		currentX = (Matrix) preX.clone();
	}
	
	/**
	 * 预测阶段
	 * 
	 * 方位角中：正北方为0度，
	 * 		   正东方90度，
	 * 		  正南方180度(-180度)
	 * 
	 * 1:初步假设手机水平向上手持平
	 * 2:
	 * 
	 * 在实验室环境中，北南方位x轴，东西方位为y轴，其中正北方为x轴正方向，正东方为y轴正方向
	 * 
	 * (0,0)							北
	 * **********************************
	 * *				*				*
	 * *				*				*
	 * *	119			*		121		*
	 * *				*				*
	 * *				*				*
	 * *				*				*
	 * ** **************************** **********************
	 * *													*
	 * ***********************************************		*
	 * *											 *		*
	 * *东											 *		*		
	 * @param azimuthAngle 方位角
	 * @param wk 角速度
	 * @param tk 步长间隔时间(s)
	 * @param sk 步长估计(m)
	 * @param Vw 角速度协方差
	 * @param Vs 步长估计协方差
	 */
	public void predict(double wk, double tk, double sk, double Vw, double Vs){
		
		double[][] tmp = new double[2][1];
//		/**
//		 * 角速度乘上时间间隔得到偏移角度加上上一步的结果
//		 */
//		tmp[0][0] = angleConvert(wk*tk + preX.get(0, 0)); 

		tmp[0][0] = sk*Math.cos(tmp[0][0]) + preX.get(1, 0);
		tmp[1][0] = sk*Math.sin(tmp[0][0]) + preX.get(2, 0);
		currentX  = new Matrix(tmp);  //得到当前的预测值
		
		/**
		 * 计算偏导部分
		 */
		double val = currentX.get(0, 0); //
		double v1 = -sk*Math.sin(val);
		double v2 = sk*Math.cos(val);
		Fk = Matrix.identity(3, 3); //一个单位矩阵
		Fk.set(1, 0, v1 + 1);
		Fk.set(2, 0, v2 + 1);
		/**
		 * 求状态噪声部分
		 */
		Qk.set(0, 0, Vw);
		Qk.set(1, 1, Vs*Math.pow(Math.cos(currentX.get(0, 0)), 2));
		Qk.set(2, 2, Vs*Math.pow(Math.sin(currentX.get(0, 0)), 2));
		
		//协方差
		currentP = Fk.times(preP).times(Fk.transpose()).plus(Qk);
	}
	
	/**
	 * 角度转换，将180-360度之间的角度转化到0到-180度
	 * 		   将大于360度的角度转化为-180到180度之间的角
	 * @param angle
	 * @return
	 */
	private double angleConvert(double angle) {
	
		boolean positive = angle >= 0 ? true : false;
		double ret = Math.abs(angle);
		double n = ret/360.0;
		if (n > 1.0) {
			ret = ret - n*360;
		} 
		if (ret > 180) {
			ret -= 360;
		}
		return (positive ? ret : -ret);
	}
	
	/**
	 * 更新
	 * @param Zk 观测值,包括航向角(由传感器得到), x,y轴（由指纹算法得到）,为3*1矩阵
	 * @param R 观测值的协方差矩阵, 为3*3矩阵
	 *
	 */
	public Matrix update(Matrix Zk, Matrix R) {
		
		Matrix t1 = H.times(currentP.inverse()).times(H.transpose()).plus(R);
		//卡尔曼增益
		Matrix K = currentP.plus(H.transpose()).times(t1.inverse());
		
		/**
		 * 更新值
		 */
		Matrix t2 = K.times(Zk.minus(H.times(currentX)));
		currentX = currentX.plus(t2);
		
		/**
		 * 更新协方差
		 */
		Matrix t3 = I3.minus(K.times(H));
		currentP = t3.times(currentP.inverse());
		
		//更新两个值
		preX = (Matrix) currentX.clone();
		preP = (Matrix) currentP.clone();
		
		return currentX;
	}
	
	
}
